/*
 * MotorController.cpp
 *
 *  Created on: Nov 13, 2013
 *      Author: Kellen Carey
 */

#include "MotorController.h"
#include <string>
#include <sstream>
#include <stdlib.h>
#include <vector>
#include <iomanip>

//#include "Dyna.h"

#include <dynamixel.h>

#define NUM_MOTORS	12

#define P_GAIN		28	// This is the address of the P value, DO NOT CHANGE

#define P_FOR_PID	32	// Change this value to change the P value
#define NEW_RETURN_DELAY_TIME	10



clock_t lastClock, batteryClock;
std::vector<Motion> motions;;
int currentMotion;

// TODO add these motor status/battery checks to the final framework
double batteryLevel= 0.00;
int temperature= 0;
int checkBattery;
int result;

int motors[NUM_MOTORS];
std::vector<int> data;

//Dyna motor_dynamixel;

const char* motion_files[] = {STAND_FILE, WALK_FILE, TURN_LEFT_FILE, TURN_RIGHT_FILE,
		SIDE_STEP_LEFT_FILE, SIDE_STEP_RIGHT_FILE, KICK_FILE};

// Default Constructor
MotorController::MotorController() {
	// Initialize USB2Dynamixel
	//	motor_dynamixel.dxl_initialize(0,1);
	dxl_initialize(0,1);
	//Initialize the motors
	initialize();
	// Set the current motion to standing
	currentMotion = STAND;

	for (int i = 0; i < NUM_MOTORS; i++) {
		motors[i] = i + 1;
	}

	data.resize(NUM_MOTORS);
}

// Default Destructor
MotorController::~MotorController() {
	//	motor_dynamixel.dxl_terminate();
	dxl_terminate();
}

//Initialize the Motions. Called from initialize()
void MotorController::initializeMotion() {

	// Define an input file stream, and open the number of motions file
	std::ifstream motionFile;

	// String variable to hold one line at a time from the text files
	std::string aString;


	// The number of steps in a particular motion
	int numMotions;

	//Open a file that specifies the number of motions we need to initialize
	motionFile.open(MOTION_FILE);

	//Let's get the total number of motion files
	//Currently this is not used, but should be later on for knowing how many motions to add to motions vector
	//TODO test this code
	getline(motionFile, aString);
	std::stringstream(aString)>>numMotions;

	numMotions = 7;

	std::cout << "Number of motions: " << numMotions <<"\n"<< std::endl;

	// Resize the motions vector
	motions.resize(numMotions);

	// Initialize each motion
	for (int i = 0; i < numMotions; i++)
	{
		motions[i] = getInitializedMotion((const char* )motion_files[i]);
	}

}

/*
 *
 * Initializes the motion associated with the file path passed in.
 * @param motion_file The file path for the motion file
 * @return tempMotion The initialized motion struct
 * Called from initializeMotion().
 */
Motion MotorController::getInitializedMotion(const char* motion_file) {

	Motion tempMotion;
	std::ifstream file;
	// Example: Open the WALK_FILE from which to read the WALK motion

	file.open(motion_file);
	int numSteps = 0;
	std::string aString;
	double tempTime;
	std::string spaceChar 	(" ");
	std::string tabChar	("\t");
	std::string enterChar	("\n");

	// Code for this motion
	getline(file, aString);				// The first number in the file is the total number of steps
	std::stringstream(aString)>>numSteps;

	std::cout << "Motion Contains "<< numSteps << " steps" <<std::endl;

	// Set the length, and resize the time and first position and velocity vectors
	tempMotion.length = numSteps;
	tempMotion.currentIndex = 0;
	tempMotion.motorPositions.resize(numSteps);
	tempMotion.motorVelocities.resize(numSteps);
	tempMotion.time.resize(numSteps);

	// Loop through the second position and velocity vectors, setting their size equal to the number of motors
	for (int i = 0; i < numSteps; i++)
	{
		tempMotion.motorPositions[i].resize(NUM_MOTORS);
		tempMotion.motorVelocities[i].resize(NUM_MOTORS);
	}

	// Initialize the time, position, and velocity data fields
	for (int i = 0; i < numSteps; i++)
	{
		// Read and store the time
		getline(file, aString);
		if (aString.find(tabChar) == 0 || aString.find(enterChar) == 0)
		{
			getline(file, aString);
		}

		std::stringstream(aString) >> tempTime;
		tempMotion.time[i] = tempTime;

		for (int j = 0; j < LEG_CUTOFF; j++)
		{
			// Initialize the motor positions and velocities from the text file
			getline(file, aString);
			if (aString.find(tabChar) == 0 || aString.find(enterChar) == 0)
			{
				getline(file, aString);
			}	

			// Find the location of the space character
			unsigned space = aString.find(tabChar);

			// Push the first number into the motor position. In [i][j], i is the step number of the motion we're in and j is the motor number
			std::stringstream(aString.substr(0, space))>> tempMotion.motorPositions[i][j];

			// Push the second number into the velocity position
			std::stringstream(aString.substr(space+1)) >> tempMotion.motorVelocities[i][j];
		}
		std::cout << "step "<< i << " built" <<std::endl;
	}
	std::cout << "Motion Built ^\n "<<std::endl;
	// Close the text file
	file.close();

	// Return the newly initialized motion
	return tempMotion;

}

// Returns the time in seconds since the lastClock parameter
float MotorController::timeSince(float lastClock) {
	// Return the number of seconds since the last clock was taken
	return (((float)(clock() - lastClock)/CLOCKS_PER_SEC));
}

/**
 *  Code to actually execute the next step
 *	called from MotorController.step(false)
 */
void MotorController::executeNextStep(Motion motion) {

	// Let the user know that the next step is being executed
	// TODO Delete this functionality for final robot
	std::cout << "Executing: " << motion.currentIndex <<  " Time since last execution = " << (float) timeSince((float) lastClock) << std::endl;
	std::cout << "Motion should take " << motion.time[motion.currentIndex] << " seconds." << std::endl;

	if (motion.currentIndex == 0)
	{
		/** Non-syncwrite
		// Set a lower acceleration speed
		for (int i = 0; i < NUM_MOTORS; i++)
		{
			motor_dynamixel.dxl_write_byte(i+1, GOAL_ACCELERATION, LOW_ACCELERATION);
		} */
		for (int i = 0; i < NUM_MOTORS; i++) {
			data[i] = LOW_ACCELERATION;
		}
		sendSyncWrite(motors, NUM_MOTORS, GOAL_ACCELERATION, BYTE, data);
	}
	else if (motion.currentIndex == 1)
	{
		/** non-syncwrite
		// Set the default acceleration
		for (int i = 0; i < NUM_MOTORS; i++)
		{
			motor_dynamixel.dxl_write_byte(i+1, GOAL_ACCELERATION, DEFAULT_ACCELERATION);
		}
		 */
		for (int i = 0; i < NUM_MOTORS; i++) {
			data[i] = DEFAULT_ACCELERATION;
		}
		sendSyncWrite(motors, NUM_MOTORS, GOAL_ACCELERATION, WORD, data);
	}
	// Write the new velocities to each of the motors
	// TODO Change this so it includes the whole body
	/**non-syncwrite
	for (int i = 0; i < LEG_CUTOFF; i++)
	{
		motor_dynamixel.dxl_write_word(i+1, MOVING_SPEED, motion.motorVelocities[motion.currentIndex][i]);
	} */
	for (int i = 0; i < NUM_MOTORS; i++) {
		data[i] = motion.motorVelocities[motion.currentIndex][i];
	}
	sendSyncWrite(motors, NUM_MOTORS, MOVING_SPEED, WORD, data);

	// Write the goal positions to each of the motors
	// TODO Change this so it includes the whole body
	/** non-syncwrite
	for (int i = 0; i < LEG_CUTOFF; i++)
	{
		motor_dynamixel.dxl_write_word(i+1, GOAL_POSITION, motion.motorPositions[motion.currentIndex][i]);
		std::cout << motion.motorPositions[motion.currentIndex][i] << "\tPresent Position: ";
		std::cout << motor_dynamixel.dxl_read_word(i+1, PRESENT_POSITION) << std::endl;
	} */
	for (int i = 0; i < NUM_MOTORS; i++) {
		data[i] = motion.motorPositions[motion.currentIndex][i];
		std::cout << motion.motorPositions[motion.currentIndex][i] << "\tPresent Position: " << std::endl;
	}
	sendSyncWrite(motors, NUM_MOTORS, GOAL_POSITION, WORD, data);

	// Reset the clock
	lastClock = clock();
}

void MotorController::initialize() {
	lastClock = clock();
	batteryClock = clock();

	changePID();
	setReturnDelayTime();

	// Cycle through leg motors, making sure to stay within the array
	//	for (int i = 0; i < LEG_CUTOFF && i < NUM_MOTORS; i++)
	//	{

	// Set a lower moving speed
	/**
	 *
	 *motor_dynamixel.dxl_write_word(i+1, MOVING_SPEED, DEFAULT_MOVING_SPEED);
	 *motor_dynamixel.dxl_write_byte(i+1, GOAL_ACCELERATION, DEFAULT_ACCELERATION);
	 */

	// Could add to set motors to standing position, but don't necessarily want to do that
	// Instead, could just assume MU-L8 is lying down, and inertialData will stand up
	//	}
	/*
	// Cycle through remaining motors
	for (int i = LEG_CUTOFF; i < NUM_MOTORS; i++)
	{

		// Set a lower moving speed for all remaining motors
		dxl_write_word(i+1, MOVING_SPEED, DEFAULT_MOVING_SPEED);
	}
	 */

	// Initialize the motion struct, most likely through reading in a text file
	initializeMotion();

	// We have to initialize the currentMotion, so we'll let it be STAND
	currentMotion = STAND;

	// Let the user know how long it's taken to initialize everything
	//std::cout << "Time since initialization: " << timeSince((float)lastClock) << "." << std::endl;

}

/*
 * Executes one non-blocking check to see if next step should be executed.
 * @param hasFallen A boolean value to check whether the robot has fallen,
 * and if so, if the proper procedure has been followed.
 * @return Returns true if motion has finished, otherwise returns false.
 */
bool MotorController::step(bool isFalling) {
	// TODO Add error checking to see if robot has fallen
	// If it hasint  fallen, make sure that it's currently standing steady

	// What we are going to return
	bool returnVar = false;

	if (isFalling)
	{
		// Relax all motors
		for (int i = 0; i < NUM_MOTORS; i++)
		{
			//			motor_dynamixel.dxl_write_byte(i+1, TORQUE_ENABLE, 0);
			dxl_write_byte(i+1, TORQUE_ENABLE, 0);
		}

		// Wait for fall to finish
		sleep(5000);

		// TODO add code for getting back up

		return false;

	}

	else //do this is if robot is not falling
	{
		// If it's been longer than the time we have to wait, let's do the next movement or step

		if (timeSince((float)lastClock) >= motions[currentMotion].time[motions[currentMotion].currentIndex-1])
		{
			//If motion is over
			if (motions[currentMotion].currentIndex == motions[currentMotion].length)
			{
				//TODO check the temp of each motor and the battery level. Battery level is not very accurate

				if(checkBattery==1){
					batteryLevel = (double) (dxl_read_byte(1, 42)*.1);
					std::cout <<std::endl<< "Battery Voltage ~ ";
					std::cout.setf( std::ios::fixed, std:: ios::floatfield ); // floatfield set to fixed
					std::cout << batteryLevel << "\n\n";

					for (int i = 0; i < NUM_MOTORS; i++)
					{
						//			motor_dynamixel.dxl_write_byte(i+1, TORQUE_ENABLE, 0);
						temperature= (int) dxl_read_byte(i+1, 43);
						std::cout <<"ID"<<i+1<<": "<< temperature <<"C	"<<std::endl;
					}
					checkBattery=0;	//don't allow the battery to be checked until it is set to 1 again(when another motion executes)
					//return voltage > VOLTAGE_THRESHHOLD;
				}
				// We have finished moving, so we'll let the caller know
				returnVar =  true;
			}
			//Motion is not over
			else
			{
				checkBattery= 1;	//lets allow the battery to be checked
				// Execute the next step
				executeNextStep(motions[currentMotion]);
				// Increment the motion counter
				motions[currentMotion].currentIndex++;
			}

			// Return the correct value;
			return returnVar;
		}
		/*
		if (timeSince((float)batteryClock) >= BATTERY_TIMEOUT)
		{
			batteryLevel = (double) dxl_read_byte(1, 42)*0.1;
			std::cout.precision(dbl::digits10);
			std::cout<<"\nBattery Voltage: " << batteryLevel/20<< "\n"<< std::endl;
			//return voltage > VOLTAGE_THRESHHOLD;

		}
		 */
		return returnVar;

	}
}
/**
 * Motion is set here from Main
 */

void MotorController::setMotion(int newMotion) {
	currentMotion = newMotion;
	motions[currentMotion].currentIndex = 0;
}

// Get the currently executing motion
int MotorController::getMotion() {
	return currentMotion;
}

void MotorController::sendSyncWrite(int ids[], int idsLength, int address, int instruction_length, std::vector<int> data) {
	// Make syncwrite packet


	dxl_set_txpacket_id(BROADCAST_ID);//ID of destination
	dxl_set_txpacket_instruction(INST_SYNC_WRITE);// packet type
	dxl_set_txpacket_parameter(0, address);//which data to manipulate. speed, goal pos, ect
	dxl_set_txpacket_parameter(1, instruction_length);//how long the instruction will be. 2 for word, 1 for byte,
	for(int i=0; i<idsLength; i++ )
	{
		// Set the ID number
		dxl_set_txpacket_parameter(2+(instruction_length+1)*i, ids[i]);
		// Set the data values
		if (instruction_length == 1) {
			dxl_set_txpacket_parameter(2+2*i+1, data[i]);
		}

		else if (instruction_length == 2) {
			dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(data[i]));
			dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(data[i]));
		}
	}
	dxl_set_txpacket_length((instruction_length+1)*idsLength+4);//(2+1) for writing a word, (1+1) for writing a byte, 4 is a constant

	dxl_txrx_packet();//sends the packet
	result = dxl_get_result( );

	if( result == COMM_TXSUCCESS )

	{
		std::cout << "COMM_TXSUCCESS "<<std::endl;
	}

	else if( result == COMM_RXSUCCESS )

	{
		std::cout << "COMM_RXSUCCESS "<<std::endl;
	}

	else if( result == COMM_TXFAIL )

	{
		std::cout << "COMM_TXFAIL "<<std::endl;
	}

	else if( result == COMM_RXFAIL)

	{
		std::cout << "COMM_RXFAIL "<<std::endl;
	}

	else if( result == COMM_TXERROR )

	{
		std::cout << "COMM_TXERROR "<<std::endl;
	}

	else if( result == COMM_RXWAITING )

	{
		std::cout << "COMM_RXWAITING "<<std::endl;
	}
}

Motion MotorController::getMotionFile() {
	return motions[getMotion()];
}

void MotorController::lockHead(){
	// TODO uncomment this if you want to lock the head
	//dxl_write_byte(24, TORQUE_ENABLE, 1);
}

bool MotorController::incrementStep() {
	// TODO Add error checking to see if robot has fallen
	// If it hasint  fallen, make sure that it's currently standing steady

	// What we are going to return
	bool returnVar1 = false;

	//If motion is over
	if (motions[currentMotion].currentIndex == motions[currentMotion].length)
	{
		returnVar1 =  true;
	}

	//Motion is not over
	else
	{

		// Execute the next step
		executeNextStep(motions[currentMotion]);
		// Increment the motion counter
		motions[currentMotion].currentIndex++;
	}

	// Return the correct value;
	return returnVar1;

}



void MotorController::incrementIndex() {
	motions[currentMotion].currentIndex++;
}









void MotorController::executePreviousStep(Motion motion) {
	// Let the user know that the next step is being executed
	// TODO Delete this functionality for final robot
	if (motion.currentIndex <= 0) { return; }
	motion.currentIndex -= 2; //This is because the currentIndex is already set to the next motion, so we need to set it to the previous motion
	std::cout << "Executing: " << motion.currentIndex <<  " Time since last execution = " << (float) timeSince((float) lastClock) << std::endl;
	std::cout << "Motion should take " << motion.time[motion.currentIndex] << " seconds." << std::endl;

	if (motion.currentIndex == 0)
	{
		for (int i = 0; i < NUM_MOTORS; i++) {
			data[i] = LOW_ACCELERATION;
		}
		sendSyncWrite(motors, NUM_MOTORS, GOAL_ACCELERATION, BYTE, data);
	}
	else if (motion.currentIndex == 1)
	{
		for (int i = 0; i < NUM_MOTORS; i++) {
			data[i] = DEFAULT_ACCELERATION;
		}
		sendSyncWrite(motors, NUM_MOTORS, GOAL_ACCELERATION, WORD, data);
	}
	// Write the new velocities to each of the motors
	// TODO Change this so it includes the whole body
	for (int i = 0; i < NUM_MOTORS; i++) {
		data[i] = motion.motorVelocities[motion.currentIndex][i];
	}
	sendSyncWrite(motors, NUM_MOTORS, MOVING_SPEED, WORD, data);

	// Write the goal positions to each of the motors
	// TODO Change this so it includes the whole body
	for (int i = 0; i < NUM_MOTORS; i++) {
		data[i] = motion.motorPositions[motion.currentIndex][i];
		std::cout << motion.motorPositions[motion.currentIndex][i] << "\tPresent Position: " << std::endl;
	}
	sendSyncWrite(motors, NUM_MOTORS, GOAL_POSITION, WORD, data);

	// Reset the clock
	lastClock = clock();

	motion.currentIndex++;
}

void MotorController::disableAllMotors() {
	std::vector<int> datas;
	datas.resize(NUM_MOTORS);
	for (int i = 0; i < NUM_MOTORS; i++) {
		datas[i] = 0;
	}

	sendSyncWrite(motors, NUM_MOTORS, TORQUE_ENABLE, BYTE, datas);
}

void MotorController::enableAllMotors() {
	std::vector<int> datas;
	datas.resize(NUM_MOTORS);
	for (int i = 0; i < NUM_MOTORS; i++) {
		datas[i] = 1;
	}
	sendSyncWrite(motors, NUM_MOTORS, TORQUE_ENABLE, BYTE, datas);
	std::cout << "\nStep\n" << motions[currentMotion].currentIndex << "\n\n";
	for (int i = 0; i < NUM_MOTORS; i++) {
//		std::cout << /*"Present Position of motor " << i+1 << ": " << */ dxl_read_word(i+1, PRESENT_POSITION) << std::endl;
	}
}

void MotorController::disableMotor(int motor) {
	dxl_write_byte(motor, TORQUE_ENABLE, 0);
}

void MotorController::enableMotor(int motor) {
	dxl_write_byte(motor, TORQUE_ENABLE, 1);
//	std::cout << "Present Position of motor " << motor << ": " << dxl_read_word(motor, PRESENT_POSITION) << std::endl;
}

void MotorController::changePID() {
	for(int i = 0; i < NUM_MOTORS; i++) {
		dxl_write_byte(i+1, P_GAIN, P_FOR_PID);
	}
}

void MotorController::disableLeftLeg() {
	int leftLeg[6];
	std::vector<int> datas;
	datas.resize(NUM_MOTORS/2);
	for (int i = 0; i < NUM_MOTORS/2; i++) {
		leftLeg[i] = 2*i + 2;
		datas[i] = 0;
	}
	sendSyncWrite(leftLeg, NUM_MOTORS/2, TORQUE_ENABLE, BYTE, datas);
}

void MotorController::disableRightLeg() {
	int rightLeg[6];
	std::vector<int> datas;
	datas.resize(NUM_MOTORS/2);
	for (int i = 0; i < NUM_MOTORS/2; i++) {
		rightLeg[i] = 2*i + 1;
		datas[i] = 0;
	}
	sendSyncWrite(rightLeg, NUM_MOTORS/2, TORQUE_ENABLE, BYTE, datas);
}

void MotorController::enableRightLeg() {
	int rightLeg[6];
	std::vector<int> datas;
	datas.resize(NUM_MOTORS/2);
	for (int i = 0; i < NUM_MOTORS/2; i++) {
		rightLeg[i] = 2*i + 1;
		datas[i] = 1;
	}
	sendSyncWrite(rightLeg, NUM_MOTORS/2, TORQUE_ENABLE, BYTE, datas);
}

void MotorController::enableLeftLeg() {
	int leftLeg[6];
	std::vector<int> datas;
	datas.resize(NUM_MOTORS/2);
	for (int i = 0; i < NUM_MOTORS/2; i++) {
		leftLeg[i] = 2*i + 2;
		datas[i] = 1;
	}
	sendSyncWrite(leftLeg, NUM_MOTORS/2, TORQUE_ENABLE, BYTE, datas);
}

int MotorController::readMotorPosition(int motor) {
	return ( (int)dxl_read_word(motor, PRESENT_POSITION) );
}

void MotorController::setReturnDelayTime() {
	std::vector<int> datas;
	datas.resize(NUM_MOTORS);
	for (int i = 0; i < NUM_MOTORS; i++) {
		datas[i] = NEW_RETURN_DELAY_TIME;
	}
	sendSyncWrite(motors, NUM_MOTORS, RETURN_DELAY_TIME, BYTE, datas);
}
